Path planning and smoothing of mobile robot based on improved artificial fish swarm algorithm

An algorithm that integrates the improved artificial fish swarm algorithm with continuous segmented Bézier curves is proposed, aiming at the path planning and smoothing of mobile robots. On the one hand, to overcome the low accuracy problems, more inflection points and relatively long planning paths in the traditional artificial fish swarm algorithm for path planning, feasible solutions and a range of step sizes are introduced based on Dijkstra's algorithm. To solve the problems of poor convergence and degradation that hinder the algorithm's ability to find the best in the later stage, a dynamic feedback horizon and an adaptive step size are introduced. On the other hand, to ensure that the planned paths are continuous in both orientation and curvature, the Bessel curve theory is introduced to smooth the planned paths. This is demonstrated through a simulation that shows the improved artificial fish swarm algorithm achieving 100% planning accuracy, while ensuring the shortest average path in the same grid environment. Additionally, the smoothed path is continuous in both orientation and curvature, which satisfies the kinematic characteristics of the mobile robot.

www.nature.com/scientificreports/ problem that the algorithm easily falls into a local optimum and is premature. In 15 , four behaviors of fish swarms were introduced with an adaptive strategy, variational strategy and hybrid strategy, and the step range and field of view range of the hyperbolic tangent function were added to optimize the optimal finding ability and improve the convergence speed, which was applied to solve the flow shop scheduling (FFS) problem 16 . The efficiency of the algorithm was proven by experiments. Additionally, the mobile robot driving path must satisfy the continuity of direction and curvature. In 17 , the path was smoothed by combining three uniform B splines. In 18 , a sequence of planned path points was fitted using the cubic Hermit spline method, which generates monotonic curves for all path points where curvature is discontinuous at the path points. However, many algorithms solve the smoothing problem either by splicing straight and circular segments or by using spline curves without considering the dynamical state of the mobile robot. The above improved artificial fish swarming algorithms all successfully solve the deficiencies of AFSA in the corresponding application areas, but some of the improved algorithms cannot reach the desired expectations in the grid environment. In the grid environment, the traditional artificial fish swarming algorithm has two types of movement: 4 movement directions with 4 neighborhoods and 8 movement directions with 8 neighborhoods. In this paper, we propose an improved artificial fish swarm algorithm with 16 moving directions and 24 neighborhoods, which makes the artificial fish have more directional choices and wider moving range during swimming. To avoid the phenomenon of individual crossing the barrier in the 16-Direction 24-Neighborhood due to the relatively large step size of the artificial fish, Dijkstra's algorithm is introduced to determine part of the step size range of the 16-Direction 24-Neighborhood again. To overcome the problem that the global optimal solution and the local optimal solution interfere with each other, the feasible solution is introduced and the optimal path is found in the feasible solution, thus, improving the planning accuracy and ensuring the shortest planning path. To overcome the problem that the computational volume increases due to the introduction of Dijkstra's algorithm, the sharing mechanism is introduced, i.e., the fish population in the same grid point can share the obstacle information in the step length range. To ensure that the planned path is more consistent with the kinematic characteristics of the mobile robot, the path smoothing algorithm is designed for the generated polyline segments after the initial path points are obtained. Thus, the generated curve is a continuous function in both orientation and curvature of the path. Finally, the feasibility and effectiveness of the algorithm are verified by simulation experiments.
Grid environment establishment. The grid graph is the most commonly used environmental modeling method in path planning. The combined algorithm of dilation and erosion of obstacles in Fig. 1a is carried out by morphology to extract features and reduce the complexity of the modeling environment to obtain Fig. 1b. This paper uses the grid method to model obstacles with 0, corresponding to black squares in the grid map, 1 representing feasible areas, and corresponding to white squares in the grid map, as shown in Fig. 2.
In the grid map, each grid has a unique coordinate location corresponding to it, and the relationship between the grid sequence number and the coordinates is as follows (1): In Eq. (1), (x, y) is the coordinate of the grid, fix() is the integral function, and mod() is the residual function. N x and N y are the maximum rows and columns of the grid map, respectively, which are equal to the maximum rows of the matrix.
In the grid map, the objective function of the optimization problem can be expressed as follows (2): The actual shape of the obstacle (b)The result after puffing treatment  www.nature.com/scientificreports/ The artificial fish school algorithm and the Dijkstra algorithm. AFSA completes the process of finding the optimal solution by simulating the preying behavior, swarming behavior, following behavior, and random behavior of fish in nature. The artificial fish swarm mainly has the following parameters: the population size Nof the fish swarm, the position X i (i = 1, 2, 3 . . . ., N) of the fish swarm, the food concentration Y c = f(x i ) of the current position of the fish swarm, the crowding factor δ of the fish swarm, the number of attempts trynumber of the fish swarm, the step size step of the fish swarm, the visual field range visual of the fish swarm, the distance between fish X i and X j is d ij =� X i − X j � , and the maximum number of iterations MAXGEN. The model is shown in Fig. 3.
Prey Behavior. Prey behavior is the most important behavior of the fish swarm algorithm. Artificial fish randomly select one location X j in the field of vision, and compares the food concentration of current location X i and location X j . If the food concentration Y j of the random position is less than the food concentration Y i of the current position, position X j will be reselected. Until the number of selections is greater than the number of attempts try-number of artificial fish, and no position with higher concentration than the current position is found, the random behavior is performed, and Eq. (3) is as follows:  www.nature.com/scientificreports/ Swarm Behavior. Swarm behavior is a substantial way of survival for fish and can be used for collective foraging, as well as for the avoidance of natural enemies. There are two necessary conditions for aggregation behavior: one is close to the center of the fish group and the other is the low crowding degree of the fish group. By observing the number n f and position X j of all fish in the field of vision, the center position X c = n f j=1 X j /n f is determined, and the concentration Y c is calculated. If the crowding degree n f /N ≤ δ of the current fish population and the concentration Y c of the center position is greater than Y i , indicating that the center position X c is better and not crowded, clustering behavior can be performed; otherwise, foraging behavior can be performed. Equation (4) is as follows: Follow Behavior. Artificial fish observe the fish within the field of vision, determine the optimal food concentration Y j and its position X j , and are close to this position. If Y j is greater than the concentration Y i of the current position and the density n f /N ≤ δ of the current fish, this shows that the better position of X j can perform the pursuit behavior; otherwise it can perform the foraging behavior. Equation (5) is as follows: Random Behavior. Random behavior is a kind of default behavior for the fish swarm. In the visual field of artificial fish, the random selection of a direction to move can prevent fish from lowing efficiency and local optimum, which is more conducive to the global optimum. The formula is expressed as follows: Dijkstra algorithm. The core idea of the Dijkstra algorithm is to take the initial point as the center point, and continuously iteratively expand to the outer layer until the target point is found. Figure 4 shows the grid map of the Dijkstra algorithm and the expression form of its adjacency matrix, and INF represents the unreachable. Improved artificial fish swarm algorithm. In the traditional AFSA, there is a problem where the global optimal solutions and the local optimal solutions interfere with each other. To avoid the algorithm falling into a local optimum, and thus, unreasonable planning occurs, the feasible solution of the fish swarm is used to replace the optimal solution, meaning that the fish swarm path point of the target point is recorded. Among these feasible solutions, the shortest path is selected as the optimal solution, and it is used as the final route of path planning.
In the grid map, the step size range [1, √ 2] of the traditional artificial fish swarm algorithm is usually 4-Direction 4-Neighborhood in Fig. 5a or 8-Direction 8-Neighborhood in Fig. 5b. However, to quickly reach the target position, it is necessary to increase the step size range. In the moving mode of 16-Direction 24-Neighborhood in this paper, the step size range of artificial fish is [1, 2 √ 2] , as shown in Fig. 6. Compared with the traditional step size range, the proposed step size range has more direction selection and a wider step size range, which is beneficial for artificial fish to quickly reach the target point. In the following, an example is used to illustrate the advantages of the proposed 16-Direction 24-Neighborhood contrast to the traditional step size range. As shown in Fig. 7, the existing fish swarm should reach X j from X i . Figure 7a, b are the paths planned by the traditional step size range. Figure 7c shows the planning path of the 16-Direction 24-Neighborhood. If the length of a grid is expressed as 1, the iteration times and path lengths of different step sizes can be calculated, as shown in Table 1. Table 1 shows that the step size range of the 16-Direction 24-Neighborhood proposed in this paper can find shorter paths with fewer iterations.    www.nature.com/scientificreports/ When the step range increases, obstacles may appear in the range, and fish can "surmount" the obstacles to reach the target point. However, for 2-D planar mobile robots, obstacle crossing must not occur. For mobile robots, the step size range proposed in this paper may lead to obstacle crossing, as shown in Fig. 8. The reason for this situation is that the traditional fish swarm algorithm determines the step size range through the Euclidean distance. However, with the increase in the step size range, the Euclidean distance cannot determine whether there is an obstacle between the two positions, and thus, obstacle crossing behavior occurs. The Dijkstra algorithm is introduced to confirm the step between two points as follows (7): If the equation is valid, there is no obstacle between the two points. Otherwise, the point does not satisfy the step range condition and is deleted from the neighborhood. However, when encountering the case in Fig. 8c, the Dijkstra algorithm is unable to identify the obstacle. Thus, formula (8)

is introduced to judge it
Allow_area_i() represents the feasible region where the step size range of the current position X i is 1, and Allow_area_j() is the feasible region where the random step size range of the neighborhood is 1, √ 2 . Dijkstra() is the Dijkstra algorithm and Dmap is the adjacency matrix of the grid map. In the traditional algorithm, the probability of each attempt in each neighborhood is the same, thereby reducing the probability of a successful attempt. Therefore, the culled factor is introduced, and the neighborhood that has been culled will no longer be tried in the next attempt.
The artificial fish wants to reach X j in the field of view, but due to the randomness of the artificial fish's swimming direction, it is necessary to determine whether the distance between the points in the neighborhood and X j is less than the distance between X i and X j , as shown in Fig. 9 below, and it is satisfied The neighborhoods of the above conditions are all filled with colors, and the remaining neighborhoods are not satisfied. In the traditional algorithm, the probability of each attempt in each neighborhood is the same, thereby reducing the probability of successful attempts. Therefore, the elimination factor is introduced, and the unsatisfactory elimination bit neighborhood will not be tried in the next attempt.
In the ASFA algorithm, the range of step size and field of vision are the main parameters affecting the convergence speed of the algorithm. The range of fields of vision has a very important influence on other behaviors of the fish swarm. The larger the range of the visual field, the more prominent the clustering behavior and following behavior of the fish swarm, which is beneficial for finding the global optimum. The small field of vision, fish foraging behavior and random behavior are more prominent, and a small range search will be more careful, but it will easily fall into a locally optimal solution. Therefore, the dynamic feedback field of vision is introduced to adjust the field of vision by feedback of the distance from the current position to the target point. Therefore, the fish can have a large field of vision in the early stage to quickly approach the target point. In the later stage, the fish can search carefully with a small field of vision and quickly reach the target point. The specific formula is as follows (9): In the formula, visual max is the maximum field of vision, m is the adjustable parameter, t is the current iteration number, T is the maximum iteration number, dist is the Euclidean distance from the target point in the fish swarm, DistMin is the Euclidean distance between the starting point and the endpoint, and λ is the feedback factor, which can be more effective in adjusting the field of vision. The larger the step range, the faster the algorithm will converge, but when it reaches a certain range, the larger the step will oscillate, which will affect the convergence speed. Therefore, an adaptive step range is introduced. When the field of vision is less than a certain value, the step range-specific formulas (10)(11) are as follows: In the formula, α is the adjustment factor, step min is the minimum step size range, and step max is the maximum step size range.
The specific steps of the improved artificial fish swarm algorithm are shown in Fig. 10: Step 1 We initialize the artificial fish parameters, such as the range of step length, range of vision, start point, end point, number of attempts, number of populations, and the crowding factor.
Step 2 We initialize the location of the fish population.
Step 3 We share the barrier information if the sharing mechanism is satisfied; otherwise, we calculate the barrier information.
Step 4 We perform preying behavior, swarming behavior, following behavior and random behavior and record the location information of the fish simultaneously.
Step 5 If the target point is found, we find the optimal solution among feasible solutions and end the loop; otherwise, we execute the next step.
Step 6 If the maximum number of cycles is reached, we end the cycle; otherwise, we return to step 3.
Path smoothing. Since the path planning in this paper was performed under the grid map, whose initial path is composed of many folded line segments, there are places with sharp turns, where curvature and path orientation discontinuity occurs. At these turning points, the continuity of curvature and path orientation must be satisfied. The relationship between the radius of the rotation and curvature is as follows (12): In this paper, the Bezier curve is used to smooth the path. In References 19,20 , the n-order Bezier curve of n + 1 control points (P0, P1, . . . , Pn) is defined as follows: In formula (17), s is 0 ≤ s ≤ 1 , and Bn, i(s) are Bernstein polynomials. www.nature.com/scientificreports/ In this paper, the third-order Bezier curve is used to smooth the path; each of the four control points controls a curve. As shown in the above figure, E 0 , B 0 , B 1 , B 2 and E 2 , A 0 , A 1 , A 2 are the points passed by the planned paths is the vector connecting E 0 and E 2 , α and β are the angles of According to the smoothing theory of continuous third-order Bezier curves, the third-order Bezier curve segments satisfy formula (14) as follows: The above curve is composed of two Bezier curves and is determined by eight control points. The first curve is controlled by four points as follows: The second curve is controlled by four points: To reduce the number of parameters, the mirror law is introduced so that d = d 1 = d 2 , α = β/2; then from Fig. 11, we obtain the Eq. (18) as follows: Substituting (18) into (14) to simplify gives Eq. (19) as follows: Substituting (18) into q a , q b , the Eqs. (20.1-20.3) obtained by simplifying as follows:  Figure 11. The Bezier curve model. In Formula (21) above, as long as the size is determined, eight control points can be determined, and the third-order Bezier curve can be constructed. In general, the first line of the two adjacent straight lines is used to construct an arc satisfying the maximum rotation curvature. At the last point A of the first arc, the first and second derivatives of the third-order Bezier curve are as follows (22): By substituting formula (22) into (12), we can obtain the equation as follows: Therefore when d ≠ 0 or α ≠ 90°, the κ max of the third-order Bezier curve is continuous. Because the planned path has multipoint collinearity, this situation does not meet the continuity condition (α ≠ 90°) of the above three-order Bezier curve, so the intermediate redundant points are removed. In this paper, the rank method of matrix is used to determine whether the three points (x 1 , y 1 ), (x 2 , y 2 ) and (x 3 , y 3 ) are linearly correlated as follows: In Eq. (24), if a is a full rank matrix, then the three points are not collinear, then (x 2 , y 2 ) is reserved, otherwise (x 2 , y 2 ) is discarded. In the algorithm, collinear optimization is carried out for the planned path. First, the number of planned path points i is calculated, and the starting point is put into Final. Then, three points P j−1 , P j , P j+1 (j = 2, 3 . . . , i − 1) are removed to judge whether they are collinear. If they are collinear, P j is put into Final, otherwise P j is discarded until the end of the j + 1 > i cycle, and finally the target point is put into Final. As shown in Fig. 12, the improved artificial fish swarm algorithm shows that the path points are (start, P i , P i+1 , P i+2 , P i+3 , goal) and that the path points after the path passing collinear optimization are (start, P i+1 , goal) , which satisfies the conditions of continuous third order Bezier curve.

Results
To verify the effectiveness and feasibility of the combination of the improved artificial fish swarm algorithm and the continuous Bessel curve smoothing algorithm proposed in this paper, comparative simulation experiments are carried out in grid environments with different complexities.
Comparison of artificial fish school algorithms. In a 10 × 10 grid map environment, several comparison experiments are carried out, and the basic parameters of the algorithm are the same, namely N = 50, try-number = 8, MAXGEN = 100, visual = 5, and ∂ = 0.618 . However, the swimming modes are 4-Direc- Figure 12. Collinear optimization of the path points.
From the analysis of Table 2, Figs. 13, 14 and 15, the path planned by ASFA-S4 is too cumbersome, with too many iterations and too much calculation. The average planning path length is relatively large and there are many inflection points; the average planned path length of ASFA-S8 is reduced by 16.5% compared with that of AFSA-S4, but the shortest and longest paths fluctuate greatly and unstable, with relatively many inflection points. The average planning path length of AFSA-S16 is 23.6% and 8.4% less than that of AFSA-S4 and AFSA-S8, respectively, with relatively fewer inflection points and the least number of operations, but it is not stable and prone to unreasonable planning, and global solution oscillation occurs due to the large range of the later field of view, which affects the algorithm's later optimization search.DAFSA-S16 introduces Dijkstra algorithm on the basis of AFSA-S16. DAFSA-S16 introduces the Dijkstra algorithm on the basis of AFSA-S16 to further confirm the obstacle information within the step length, and the simulation results show that the accuracy of the algorithm is improved. At the same time, the average path length of DAFSA-S16 is reduced by 24.3%, 9.3%, and 1.0% compared with AFSA-S4, AFSA-S8, and AFSA-S16, respectively, and the inflection point is also relatively small.  www.nature.com/scientificreports/ Since the large field of vision in the late stage will affect the optimization of the algorithm, the late oscillation of the algorithm shown in Fig. 13d above appears. IDSFA-S16 can effectively solve the behavior of unreasonable crossing of the planning path, while the feedback field of view range can effectively solve the problem of global solution oscillation at the later stage of the algorithm. The average path length is 27.4%, 13%, 5.0%, 4.0% less than the above algorithm, and the accuracy of the algorithm planning can also reach 100%, while the inflection point is relatively less, and the path is smoother, but the amount of operations is relatively increased.   www.nature.com/scientificreports/ the improved DAFSA-S16 algorithm (IDFSA-S16) proposed in this paper. The basic parameters of IDAFSA-S16 are visual max = 5, step max = 2 √ 2, step min = √ 2 , and m = 10. Table 3 shows the comparison of planned path lengths in a 20 × 20 grid environment, Fig. 16 shows the planning path under the grid environment of 20 × 20, and Fig. 17 shows the comparison iteration diagram.
The analysis of Table 3, Figs. 16, 17 and 18 shows that in the relatively complex environment, the path planned by AFSA-S4 is very complex, with the largest number of inflection points and iterations, as well as the largest number of algorithm operations. There is also an unreasonable cross-barrier phenomenon. The difference between the longest path and the shortest path planned by ASFA-S8 is large, and the global optimal solution and the local optimal solution interfere with each other, leading to unreasonable planning paths. AFSA-S16 is difficult to plan the route correctly. DAFSA-S16 planning accuracy is improved, and the inflection point is relatively small, but it is also difficult to control the local optimum and the global optimum interfering with each other. By replacing the optimal solution with a feasible solution, IDAFSA-S16 can ensure rationality and stability under the planned path, while also reducing the number of inflection points and the number of iterations. Then, the algorithm operations are relatively reduced, and the average path lengths are 25.4%, 3.9%, 0.5% and 0.2% less than those of the above algorithms.

Path Smoothing.
To verify the effectiveness of the path smoothing of the third-order Bessel curve and the continuity after collinear optimization, simulations are carried out in grid environments with different com-  Fig. 19. From the simulation results, it can be seen that in different environments, by introducing the third-order Bessel curve, the path is smoothed so that the original sharp inflection point becomes smooth, and the actual control meets the kinematic and dynamic characteristics of the mobile robot. By effectively avoiding obstacles and satisfying feasibility, the continuity of the rotation angle and path curvature of the mobile robot is realized.

Discussion and conclusion
Aiming at robot path planning, this paper proposed a method combining third-order Bessel curves with an improved artificial fish swarm algorithm. First, morphological processing, such as the expansion of obstacles, was carried out, and during the path planning process, an artificial fish swimming range of 16 directions and 24 fields based on Dijkstra's algorithm was introduced to provide the movement rules of the artificial fish, which  www.nature.com/scientificreports/ improves the accuracy of planning while reducing the inflection points. At the same time, by adding the sharing mechanism, the number of operations of the fusion algorithm was reduced. By introducing the feasible solution, the problem of mutual interference between the local optimal solution and the global optimal solution was solved. By introducing the feedback field of view range, the oscillation phenomenon caused by an excessively large field of view in the later stage of the algorithm was avoided. The improved artificial fish swarm algorithm proposed in this paper generates a collision-free, inflection-point less and shorter path connected by a sequential sequence of path points. Finally, smoothing by using the theory of cubic Bessel curves ensures the continuity of direction and curvature of any point on its path, while made it satisfy the minimum rotation radius to reduce the mechanical structure damage of the robot. By comparing the simulation results of the proposed algorithm with the traditional fish swarm algorithm, it can be seen that the proposed fusion algorithm had the shortest average path, the least number of inflection points, and was more consistent with the kinematic characteristics of the robot, while also ensured a 100% correct planning rate. At the same time, the proposed fusion algorithm introduces Dijkstra's algorithm, which causes an increase in computation, but then the fish sharing mechanism was introduced to offset part of the computation. In summary, it is sufficient to prove the effectiveness and superiority of the proposed algorithm in this paper. However, the proposed algorithm was only tested in a raster environment and not in a real environment. In future work, we will conduct experiments and debugging in a real environment. We are also interested in using IDAFSA in multirobot collaborative path planning.